
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_takeoff.c
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

/*
  return a tail hold percentage during initial takeoff for a tail
  dragger

  This can be used either in auto-takeoff or in FBWA mode with
  FBWA_TDRAG_CHAN enabled
 */
int8_t fms_takeoff_tail_hold(void)
{
    bool in_takeoff = ((fms.control_mode == (mode_base_t)&fms.mode_auto && !fms.auto_state.takeoff_complete) ||
                       (fms.control_mode == (mode_base_t)&fms.mode_fbwa && fms.auto_state.fbwa_tdrag_takeoff_mode));
    if (!in_takeoff) {
        // not in takeoff
        return 0;
    }
    if (fms.g.takeoff_tdrag_elevator == 0) {
        // no takeoff elevator set
        goto return_zero;
    }
    if (fms.auto_state.highest_airspeed >= fms.g.takeoff_tdrag_speed1) {
        // we've passed speed1. We now raise the tail and aim for
        // level pitch. Return 0 meaning no fixed elevator setting
        goto return_zero;
    }
    if (fms.ahrs->pitch_sensor_cd > fms.auto_state.initial_pitch_cd + 1000) {
        // the pitch has gone up by more then 10 degrees over the
        // initial pitch. This may mean the nose is coming up for an
        // early liftoff, perhaps due to a bad setting of
        // g.takeoff_tdrag_speed1. Go to level flight to prevent a
        // stall
        goto return_zero;
    }
    // we are holding the tail down
    return fms.g.takeoff_tdrag_elevator;

return_zero:
    if (fms.auto_state.fbwa_tdrag_takeoff_mode) {
        gcs_send_text(MAV_SEVERITY_NOTICE, "FBWA tdrag off");
        fms.auto_state.fbwa_tdrag_takeoff_mode = false;
    }
    return 0;
}
/*------------------------------------test------------------------------------*/


